#include <iostream>
#include <fstream>
#include <chrono>
#include <algorithm>
#include <opencv2/opencv.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/xfeatures2d.hpp>
#include "optical_flow.h"
#include <unistd.h>




using namespace std;
using namespace cv;


int main()
{
	cv::VideoCapture capture("/home/wenyi/workspace/200-test/4.h264");

	if (!capture.isOpened())
	{
		cout << "video file open error." << endl;
		return -1;
	}

	int width = capture.get(cv::CAP_PROP_FRAME_WIDTH);
	int height = capture.get(cv::CAP_PROP_FRAME_HEIGHT);
	int count = capture.get(cv::CAP_PROP_FRAME_COUNT);
	double fps = capture.get(cv::CAP_PROP_FPS);
	cout << "resolution: " << width << " * " << height << endl;
	cout << "count: " << count << endl;
	cout << "fps: " << fps << endl;
	double interval = 1.0 / fps;
	cout << "interval: " << interval << endl;

	cv::Mat im(cv::Size(width, height), CV_16UC4);
	cv::Mat imGray;
	cv::Mat prevImGray;
	cv::Mat lastImGray;
	bool firstImage = false;

	uint8_t prev[width * height];
	uint8_t last[width * height];

	//original focal lenght: 12mm pixelsize: 6um, binning 4 enabled
	const float focal_length_px = PARAM_FOCAL_LENGTH_MM / (4.0f * 6.0f) * 1000.0f; 

	int64 startTime = getTickCount();
	int64 delay;

	float alt = 120.0f; //120m
	bool distance = true;

	float velocity_x_lp = 0.0f;
	float velocity_y_lp = 0.0f;

	while(1)
	{
		capture >> im;
		alt -= 3 / fps;

		alt = alt > 0 ? alt : 1;

		int64 t1 = getTickCount();

		if(((t1 - startTime) / getTickFrequency()) < 13)
		{
			//33ms
			delay = interval*1000;
			usleep(delay);
			continue;
		}

		if (nullptr == im.ptr() || 0 == im.rows || 0 == im.cols)
		{
			break;
		}

		cvtColor(im, imGray, CV_BGR2GRAY);


		if(imGray.empty())
		{
			cout << "image empty" << endl;
			break;
		}

		if(!firstImage)
		{
			firstImage = true;
			prevImGray = imGray.clone();
			continue;
		}

		lastImGray = imGray.clone();

		//compute optical flow
		uint8_t qual = 0;
		float pixel_flow_x = 0.0f;
		float pixel_flow_y = 0.0f;

		for(int i=0; i<height; i++)
		{
			for(int j=0; j<width; j++)
			{
				prev[i * width + j] = prevImGray.at<uchar>(i,j);
				last[i * width + j] = lastImGray.at<uchar>(i,j);
			}
		}

		qual = compute_flow(prev, last, 0, 0, 0, &pixel_flow_x, &pixel_flow_y);


		//cout << "qual:"<< (int)qual <<",pixel flow x:" << pixel_flow_x << ",pixel flow y:" << pixel_flow_y << endl;

		
		// real point P (X,Y,Z), image plane projection p (x,y,z), focal-length f, distance-to-scene Z
		// x / f = X / Z
		// y / f = Y / Z

		float flow_compx = pixel_flow_x / focal_length_px / TIME_BETWEEN_IMAGES;
		float flow_compy = pixel_flow_y / focal_length_px / TIME_BETWEEN_IMAGES;


		if(distance)
		{
			/* calc velocity (negative of flow values scaled with distance) */
			float new_velocity_x = - flow_compx * alt;
			float new_velocity_y = - flow_compy * alt;

			/* integrate velocity and output values only if distance is valid */
			if(qual > 0)
			{
				/* lowpass velocity output */
				velocity_x_lp = PARAM_BOTTOM_FLOW_WEIGHT_NEW * new_velocity_x +
							(1.0f - PARAM_BOTTOM_FLOW_WEIGHT_NEW) * velocity_x_lp;
				velocity_y_lp = PARAM_BOTTOM_FLOW_WEIGHT_NEW * new_velocity_y +
							(1.0f - PARAM_BOTTOM_FLOW_WEIGHT_NEW) * velocity_y_lp;


				cout << "velocity_x_lp:" << velocity_x_lp << ",velocity_y_lp:" << velocity_y_lp << endl;
				
			}
			else
			{
				/* taking flow as zero */
				velocity_x_lp = (1.0f - PARAM_BOTTOM_FLOW_WEIGHT_NEW) * velocity_x_lp;
				velocity_y_lp = (1.0f - PARAM_BOTTOM_FLOW_WEIGHT_NEW) * velocity_y_lp;
			}
		}
		else
		{
			/* taking flow as zero */
			velocity_x_lp = (1.0f - PARAM_BOTTOM_FLOW_WEIGHT_NEW) * velocity_x_lp;
			velocity_y_lp = (1.0f - PARAM_BOTTOM_FLOW_WEIGHT_NEW) * velocity_y_lp;
		}
		

		prevImGray = imGray.clone();

		int64 t2 = getTickCount();

		int64  useTimeMs = (t2 - t1) * 1000 / getTickFrequency();

		cout << "use time:" << useTimeMs << " ms" << endl;

		imshow("video", im);
		
		delay = interval*1000 - useTimeMs;
		delay = delay <= 0 ? 1 : delay;
		//usleep(delay);
		waitKey(delay);
	}

	return 0;
}

